#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

int main() {
    // Load image
    cv::Mat image = cv::imread("../pic.png", cv::IMREAD_GRAYSCALE);
    if (image.empty()) {
        std::cerr << "Failed to load image!" << std::endl;
        return -1;
    }

    // Convert OpenCV Mat to Eigen matrix
    Eigen::MatrixXd eigenMat;
    cv::cv2eigen(image, eigenMat);

    // Increase canvas size to accommodate rotation, setting background to white (255)
    int newRows = eigenMat.rows() * 2;
    int newCols = eigenMat.cols() * 2;
    Eigen::MatrixXd workMat = Eigen::MatrixXd::Constant(newRows, newCols, 255.0);

    // Place the original image in the center of the work matrix
    int rowOffset = (newRows - eigenMat.rows()) / 2;
    int colOffset = (newCols - eigenMat.cols()) / 2;
    workMat.block(rowOffset, colOffset, eigenMat.rows(), eigenMat.cols()) = eigenMat;

    // Create rotation matrix (15 degrees counter-clockwise)
    double angle = 15.0 * M_PI / 180.0;
    Eigen::Matrix2d rotationMatrix;
    rotationMatrix << cos(angle), -sin(angle),
                      sin(angle),  cos(angle);

    // Create rotated matrix with white background
    Eigen::MatrixXd rotatedMat = Eigen::MatrixXd::Constant(newRows, newCols, 255.0);
    int cx = workMat.cols() / 2;
    int cy = workMat.rows() / 2;

    for (int y = 0; y < workMat.rows(); ++y) {
        for (int x = 0; x < workMat.cols(); ++x) {
            Eigen::Vector2d pos(x - cx, y - cy);
            Eigen::Vector2d newPos = rotationMatrix * pos;

            int newX = static_cast<int>(std::round(newPos.x())) + cx;
            int newY = static_cast<int>(std::round(newPos.y())) + cy;

            if (newX >= 0 && newX < workMat.cols() && newY >= 0 && newY < workMat.rows()) {
                rotatedMat(y, x) = workMat(newY, newX);
            }
        }
    }

    // Convert Eigen matrix back to OpenCV Mat
    cv::Mat outputMat;
    cv::eigen2cv(rotatedMat, outputMat);
    outputMat.convertTo(outputMat, CV_8U);

    // Display the rotated image
    cv::imshow("Rotated Image", outputMat);
    cv::waitKey(0);

    return 0;
}
